#include "filter.h"


int16_t filter_arr_tmp[7][FILTER_NUM];
static uint8_t filter_index;

Mpu_filter_data_t mpu_filter_data;


void mpu_average_filter(){
	uint8_t i;

	filter_arr_tmp[0][filter_index] = mpu_data.ax;
	filter_arr_tmp[1][filter_index] = mpu_data.ay;
	filter_arr_tmp[2][filter_index] = mpu_data.az;
	filter_arr_tmp[3][filter_index] = mpu_data.gx;
	filter_arr_tmp[4][filter_index] = mpu_data.ay;
	filter_arr_tmp[5][filter_index] = mpu_data.gz;
	filter_arr_tmp[6][filter_index] = mpu_data.temperature;
	
	filter_index = (filter_index == FILTER_NUM - 1) ? 0 : filter_index + 1;
	
	int32_t average_num = 0;

	for (i = 0; i < FILTER_NUM; i++){
		average_num += filter_arr_tmp[0][i];
	}
	mpu_filter_data.ax = average_num / FILTER_NUM;
	average_num = 0;

	for (i = 0; i < FILTER_NUM; i++){
		average_num += filter_arr_tmp[1][i];
	}
	mpu_filter_data.ay = average_num / FILTER_NUM;
	average_num = 0;

	for (i = 0; i < FILTER_NUM; i++){
		average_num += filter_arr_tmp[2][i];
	}
	mpu_filter_data.az = average_num / FILTER_NUM;
	average_num = 0;

	for (i = 0; i < FILTER_NUM; i++){
		average_num += filter_arr_tmp[3][i];
	}
	mpu_filter_data.gx = average_num / FILTER_NUM;
	average_num = 0;

	for (i = 0; i < FILTER_NUM; i++){
		average_num += filter_arr_tmp[4][i];
	}
	mpu_filter_data.gy = average_num / FILTER_NUM;
	average_num = 0;

	for (i = 0; i < FILTER_NUM; i++){
		average_num += filter_arr_tmp[5][i];
	}
	mpu_filter_data.gz = average_num / FILTER_NUM;
	average_num = 0;

	for (i = 0; i < FILTER_NUM; i++){
		average_num += filter_arr_tmp[6][i];
	}
	mpu_filter_data.temperature = average_num / FILTER_NUM;
	average_num = 0;
}


uint8_t get_status(){
	int tmp = 0;
	tmp = filter_arr_tmp[1][0] + filter_arr_tmp[1][1] + filter_arr_tmp[1][2] + filter_arr_tmp[1][3] + filter_arr_tmp[1][4] + filter_arr_tmp[1][5] + filter_arr_tmp[1][6];
	if (tmp > 42000)
		return 1;
	tmp = filter_arr_tmp[0][0] + filter_arr_tmp[0][1] + filter_arr_tmp[0][2] + filter_arr_tmp[0][3] + filter_arr_tmp[0][4] + filter_arr_tmp[0][5] + filter_arr_tmp[0][6];
	if (tmp > 42000)
		return 2;
	else if (tmp < -21000)
		return 3;
	else
		return 0;
}